#include <iostream>

Vector3 getTrackballVector(int cx, int cy);
void mouseFunc(int button, int state, int x, int y);
void motionFunc(int cx, int cy);
int lastX, lastY;
Matrix4 *rot;
Matrix4 *mat;
int Twidth = 512;
int Theight = 512;

void trackballInit(int w, int h) {
	rot = new Matrix4();
	rot->identity();
	mat = new Matrix4();
	Twidth = w;
	Theight = h;
}

void mouseFunc(int button, int state, int x, int y) {
	lastX = x;
	lastY = y;
}

void motionFunc(int cx, int cy) {
	Vector3 v1 = getTrackballVector(lastX, lastY);
	Vector3 v2 = getTrackballVector(cx, cy);
	Vector3 axis;
	axis.cross(v1, v2);
	double theta = v1.angleBetween(v1, v2);
	mat->rotationArbitrary(axis, -theta / 10);
	mat->multiply(*rot);
	rot->set(mat->get());
}

Vector3 getTrackballVector(int cx, int cy) {
	float w = Twidth;
	float h = Theight;
	double x = cx / (w / 2);
	double y = cy / (h / 2);
	x = x - 1;
	y = 1 - y;
	double z2 = 1 - x * x - y * y;
	double z = z2 > 0 ? sqrt(z2) : 0;
	Vector3 v;
	v.set(x, y, z);
	v.normalize();
	return v;
}
